﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace HubertTestProgram
{
    public class BasicStamp
    {

        #region Constants
        private enum CommandIndex   // Interpreted by the BS2 microcontroller: See Hubert.bs2
        {
           DoNothing = 0,
           SetServoPosition = 1,
           ReturnToInitialPosition = 99
           // Add other commands here if any (e.g. for sensor reading)
        }
        #endregion

        #region Fields
        private string portName;
        private SerialCommunication serialCommunication;
        private Dictionary<byte, short> lastCommandSent;
        #endregion

        #region Private Methods
        private bool isNewCommand(ServoMotor servoMotor, short newPositionValue)
        {
            short oldPositionValue;
            if (lastCommandSent.TryGetValue(servoMotor.ChannelIndex, out oldPositionValue))
            {
                if (oldPositionValue == newPositionValue)
                {
                    return false;
                }
                else //new command
                {
                    lastCommandSent[servoMotor.ChannelIndex] = newPositionValue;
                    return true;
                }
            }
            else //No command sent to servo => new command
            {
                lastCommandSent.Add(servoMotor.ChannelIndex, newPositionValue);
                return true;
            }
        }
        #endregion

        #region Public methods
        public void Initialize()
        {
            serialCommunication = new SerialCommunication(portName);
        }

        public BasicStamp()
        {
            lastCommandSent = new Dictionary<byte, short>();
        }

        public void SetServoPosition(ServoMotor servoMotor, short positionValue, byte servoSpeed)
        {
            if (isNewCommand(servoMotor, positionValue))
            {
                byte[] dataToSend = new byte[5];
                byte positionLowByte = (byte)positionValue;
                byte positionHighByte = (byte)(positionValue >> 8);
                dataToSend[0] = (byte)CommandIndex.SetServoPosition;
                dataToSend[1] = servoMotor.ChannelIndex;
                dataToSend[2] = positionLowByte;
                dataToSend[3] = positionHighByte;
                dataToSend[4] = servoSpeed;
                serialCommunication.SendCommand(dataToSend);
            }
        }

        public void ReturnToInitialPosition()
        {
            byte[] dataToSend = new byte[5];
            dataToSend[0] = (byte)CommandIndex.ReturnToInitialPosition;
            dataToSend[1] = 0;  // not really needed..
            dataToSend[2] = 0;
            dataToSend[3] = 0;
            dataToSend[4] = 0;
            serialCommunication.SendCommand(dataToSend);
        }

        public void Disconnect()
        {
            if (serialCommunication != null)
            {
                serialCommunication.DisposeConnection();
            }
        }
        #endregion

        #region Properties
        public string PortName
        {
            get { return portName; }
            set { portName = value; }
        }
        public SerialCommunication SerialCommunication
        {
            get { return serialCommunication; }
        }
        #endregion
    }
}
